#include<stdio.h>
#include<string.h>
#include<stdlib.h>
#include<assert.h>
#include<math.h>

//画像のピクセル値をとっておくための構造体。
typedef struct greyImagePix{
	int width;
	int height;
	int* pix;
}* GreyImagePix;
GreyImagePix mallocGreyImage(width,height){
	GreyImagePix ret=(GreyImagePix)malloc(sizeof(struct greyImagePix)); assert(ret!=NULL);
	ret->width=width;
	ret->height=height;
	ret->pix=(int*)malloc(sizeof(int)*width*height); assert(ret->pix!=NULL);
	return ret;
}
void freeGreyImage(GreyImagePix im){
	free(im->pix);
	free(im);
}
float* calcHOG(GreyImagePix img,int cellRow,int cellCol,int blockWidth,int blockHeight,int histgramDimension){
    int cellWidth=img->width/cellCol;
    int cellHeight=img->height/cellRow;
    float* histgram=(float*)malloc(sizeof(float)*histgramDimension); assert(histgram!=NULL);
    float* cellFeatureVector=(float*)malloc(sizeof(float)*histgramDimension*cellCol*cellRow); assert(cellFeatureVector!=NULL);
    int cx,cy,u,v,d;
    
    for(cy=0;cy<cellRow;cy++){
        for(cx=0;cx<cellCol;cx++){
            memset(histgram,0,sizeof(float)*histgramDimension);//ゼロクリアするタイミングはここ
            for(v=0;v<cellHeight;v++){
                int y=cy*cellHeight+v;
                int ymm=(y==0)? 0 : y-1;
                int ypp=(y==img->height-1)? y : y+1;
                for(u=0;u<cellWidth;u++){
                    int x=cx*cellWidth+u;
                    int xmm=(x==0)? 0 : x-1;
                    int xpp=(x==img->width-1)? x : x+1;
                    
                    int dy=img->pix[ypp*img->width+x]-img->pix[ymm*img->width+x];
                    int dx=img->pix[y*img->width+xpp]-img->pix[y*img->width+xmm];
                    
                    int idx=(int)(0.5*(atan2(dy,dx)/M_PI + 1)*(histgramDimension-1));
                    histgram[idx]+=sqrt(dx*dx+dy*dy);
                }
            }
            
            for(d=0;d<histgramDimension;d++) cellFeatureVector[(d*cellRow+cy)*cellCol+cx]=histgram[d];
        }
    }
    free(histgram);
    
    int blockDimension=histgramDimension*blockWidth*blockHeight;
    float* blockFeatureVector=(float*)malloc(sizeof(float)*blockDimension); assert(blockFeatureVector!=NULL);
    float* ret=(float*)malloc(sizeof(float)*histgramDimension*blockWidth*blockHeight*(cellRow-blockHeight)*(cellCol-blockWidth)); assert(ret!=NULL);
    
    for(cy=0;cy<cellRow-blockHeight;cy++){
        for(cx=0;cx<cellCol-blockWidth;cx++){
            float norm=0;
            for(v=0;v<blockHeight;v++){
                for(u=0;u<blockWidth;u++){
                    for(d=0;d<histgramDimension;d++){
                        float _v=cellFeatureVector[(d*cellRow+cy+v)*cellCol+cx+u];
                        blockFeatureVector[(d*blockHeight+v)*blockWidth+u]=_v;
                        norm+=_v*_v;
                    }
                }
            }
            norm=sqrt(norm+1);
            for(d=0;d<blockDimension;d++){
                blockFeatureVector[d]/=norm;
                ret[(d*(cellRow-blockHeight)+cy)*(cellCol-blockWidth)+cx]=blockFeatureVector[d];
            }
        }
    }

    free(cellFeatureVector);
    free(blockFeatureVector);

    return ret;
}

void freeHOG(float* hogResult){
    free(hogResult);
}